/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: utilities.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <pthread.h>
#include <math.h>

#include "globaldefs.h"
#include "utilities.h"
#include "matrix.h"

#ifdef NCURSE_DISPLAY_OPTION
//#include <curses.h>
//extern WINDOW *win;
//static void set_colors(int pair, int foreground, int background);
//static chtype use_colors(int pairs, chtype attrs);
#endif

void displayMessage(struct imu *imuData,struct gps *gpsData)
{ 

#ifndef DEBUG
	imuData->time=getTime();
#endif
#ifndef NCURSE_DISPLAY_OPTION
	printf("[m/s^2]:ax=%+6.6f ay=%+6.6f az=%+6.6f \n",imuData->ax,imuData->ay,imuData->az);
	printf("[m/s]:dvx=%6.6f dvy=%6.6f dvz=%6.6f \n",imuData->dvx,imuData->dvy,imuData->dvz);
	printf("[rad/s]:p=%6.6f q=%6.6f r=%6.6f \n",imuData->p, imuData->q, imuData->r);
	printf("[rad]:dthetax=%6.6f dthetay=%6.6f dthetaz=%2.6f \n",imuData->dthetax,imuData->dthetay,imuData->dthetaz);
	printf("[DCM]:T11=%6.3f T12=%6.3f  T13=%6.3f  \n",imuData->T11,imuData->T12,imuData->T13);
	printf("[DCM]:T21=%6.3f T22=%6.3f  T23=%6.3f  \n",imuData->T21,imuData->T22,imuData->T23);
	printf("[DCM]:T31=%6.3f T32=%6.3f  T33=%6.3f  \n",imuData->T31,imuData->T32,imuData->T33);
	printf("[DeltaDCM]:DT11=%6.3f DT12=%6.3f  DT13=%6.3f  \n",imuData->DT11,imuData->DT12,imuData->DT13);
	printf("[DeltaDCM]:DT11=%6.3f DT12=%6.3f  DT13=%6.3f  \n",imuData->DT21,imuData->DT22,imuData->DT23);
	printf("[DeltaDCM]:DT11=%6.3f DT12=%6.3f  DT13=%6.3f  \n",imuData->DT31,imuData->DT32,imuData->DT33);
	printf("[sec]:timer=%lu \n",imuData->timer_imu);
	printf("[deg]:pitch=%6.3f  \n",asin(-1*imuData->T13)*R2D);
	printf("[deg]:roll=%6.3f  \n",atan2(imuData->T23,imuData->T33)*R2D);
	printf("[deg]:yaw=%6.3f  \n",atan2(imuData->T12,imuData->T11)*R2D);
	printf("[seconds]:time=%6f  \n",imuData->time);
	printf("[GPS]:lat=%6.3f lon=%6.3f alt=%6.3f \n",gpsData->lat,gpsData->lon,gpsData->alt_msl);
	printf("[GPS]:vn=%6.3f ve=%6.3f vd=%6.3f \n",gpsData->vn,gpsData->ve,gpsData->vd);
#endif
	
/*#ifdef NCURSE_DISPLAY_OPTION
	int width,height;
    char buf[100];
	
	box(win, ACS_VLINE, ACS_HLINE);
	set_colors(2,COLOR_WHITE,COLOR_BLACK);
    mvwaddstr(win, 0, 4," Onboard Flight Control System Information ");
    	set_colors(3,COLOR_GREEN,COLOR_BLACK);
    mvwaddstr(win, 2, 4,">> All Systems Test V0.2:");    	
    
	set_colors(2,COLOR_WHITE,COLOR_BLACK);
	sprintf(buf,"[ rad ]:phi = %6.3f the = %6.3f   psi = %6.3f",imuData->roll,imuData->pitch,imuData->yaw);
	mvwaddstr(win, 3, 4,buf);    	
	sprintf(buf,"[rad/s]:p   = %6.3f q   = %6.3f   r   = %6.3f",imuData->p, imuData->q, imuData->r);
	mvwaddstr(win, 4, 4,buf);      	 
	sprintf(buf,"[m/s^2]:ax  = %6.3f ay  = %6.3f   az  = %6.3f",imuData->ax,imuData->ay,imuData->az);
	mvwaddstr(win, 5, 4,buf); 
    sprintf(buf,"[ m/s ]:dvx = %6.3f dvy = %6.3f   dvz = %6.3f",imuData->dvx,imuData->dvy,imuData->dvz);
	mvwaddstr(win, 6, 4,buf);
	sprintf(buf,"[ rad ]:dtx = %6.3f dty = %6.3f   dtz = %6.3f",imuData->dthetax,imuData->dthetay,imuData->dthetaz);
	mvwaddstr(win, 7, 4,buf);
	sprintf(buf,"[ GPS ]:lat = %6.3f lon = %6.3f  alt = %6.3f",gpsData->lat,gpsData->lon,gpsData->alt_msl);
	mvwaddstr(win, 8, 4,buf);
	sprintf(buf,"[ GPS ]:vn  = %6.3f ve  = %6.3f   vd  = %6.3f",gpsData->vn,gpsData->ve,gpsData->vd);
	mvwaddstr(win, 9, 4,buf);
    sprintf(buf,"[ sec ]:t   = %6.3f",imuData->time);
	mvwaddstr(win, 10, 4,buf);  
    
	set_colors(3,COLOR_GREEN,COLOR_BLACK);
	mvwaddstr(win, 22, 3," Ctrl+C to Quit ");
    
	wrefresh(win); 
    nodelay(win,TRUE);	
	
	
#endif*/
}	
float floatFromBytesBE(const byte* buffer)
{
	float f = 0;
	
	((byte*)(&f))[0] = buffer[3];
	((byte*)(&f))[1] = buffer[2];
	((byte*)(&f))[2] = buffer[1];
	((byte*)(&f))[3] = buffer[0];
	

	return f; 
}
float floatFromBytesLE(const byte* buffer)
{
	float f = 0;
	
	((byte*)(&f))[0] = buffer[0];
	((byte*)(&f))[1] = buffer[1];
	((byte*)(&f))[2] = buffer[2];
	((byte*)(&f))[3] = buffer[3];
	

	return f; 
}
void byteFromFloatLE(byte *buffer,const float* f)
{
	(buffer)[3] = ((byte*)(&f))[3];
	(buffer)[2] = ((byte*)(&f))[2];
	(buffer)[1] = ((byte*)(&f))[1];
	(buffer)[0] = ((byte*)(&f))[0];
}

unsigned long longFromBytesBE(const byte* buffer)
{
	unsigned long l = 0;
	
	((byte*)(&l))[0] = buffer[3];
	((byte*)(&l))[1] = buffer[2];
	((byte*)(&l))[2] = buffer[1];
	((byte*)(&l))[3] = buffer[0];
	

	return l; 
}

signed long longFromBytesLE(const byte* buffer)
{
	signed long l = 0;
	
	((byte*)(&l))[0] = buffer[0];
	((byte*)(&l))[1] = buffer[1];
	((byte*)(&l))[2] = buffer[2];
	((byte*)(&l))[3] = buffer[3];
	

	return l; 
}

unsigned short uSintFromBytesBE(byte* buffer)
{
	unsigned short uSi = 0;
	
	((byte*)(&uSi))[0] = buffer[1];
	((byte*)(&uSi))[1] = buffer[0];
	
	return uSi; 
}

signed int sIntFromBytesBE(byte* buffer)
{
	signed int sI = 0;
	
	((byte*)(&sI))[0] = buffer[3];
	((byte*)(&sI))[1] = buffer[2];
	((byte*)(&sI))[2] = buffer[1];
	((byte*)(&sI))[3] = buffer[0];
	
	return sI; 
}

void dataLogging(struct imu *imuData,struct gps *gpsData)
{
	FILE   	*fimu,*fgps;
    
	/*********************************************************************
   *Open Files
   *********************************************************************/
 	if((fimu = fopen("data/imu.dat","a+b"))==NULL) {
            printf("imu.dat cannot be created in /imuData directory...error!\n");
            _exit(-1);
         }
         else
         fwrite(imuData,sizeof(struct imu),1,fimu);
 	
 	if((fgps = fopen("data/gps.dat","a+b"))==NULL) {
            printf("gps.dat cannot be created in /gpsData directory...error!\n");
            _exit(-1);
         }
         else
 			fwrite(gpsData,sizeof(struct gps),1,fgps);
 	
 	
 	fclose(fimu);
 	fclose(fgps);
}

/***************************************************************************************
 *check the checksum of the imuData packet
 ***************************************************************************************/
int checksumCheck(byte* buffer, int packet_len)
{
   word     	 i=0,rcvchecksum=0;
   word          sum=0;
 
   for(i=0;i<packet_len-2;i++) 
   		sum = sum + buffer[i];
   		rcvchecksum = buffer[packet_len-2];
   		rcvchecksum = (rcvchecksum <<8) | buffer[packet_len-1];
   if (rcvchecksum == sum)
   	return    TRUE;
   else
 	return    FALSE;
}
void createChecksum(byte *buffer, int packet_len)
{
	byte chk0 = 0;
	byte chk1 = 0;
	int i;
	for(i=2;i<packet_len-2;i++)
	{
		chk0 = chk0 + buffer[i];
		chk1 = chk1 + chk0;
	}
	buffer[packet_len-2] = chk1;
	buffer[packet_len-1] = chk0;
}
void snap_time_interval(char *threadname,int displaytime,short id)
{
	static int				count[5]={0,};
	struct 	timespec		ts;
    static struct timespec  ts_p[5];
    double 					sec,nsec,elapsed;
    static double           sum[5]={0.,};
    double 					exe_rate[3];
    
	clock_gettime(CLOCK_REALTIME, &ts);
    sec 	= ts.tv_sec - ts_p[id].tv_sec;
    nsec    = ts.tv_nsec- ts_p[id].tv_nsec;
    elapsed = sec + nsec*1.0e-9;
    ts_p[id]= ts;
    sum[id]+= elapsed;
	
   	if (++count[id] == displaytime)
	{       elapsed = sum[id]/displaytime; sum[id] = 0;
	        exe_rate[id]=elapsed;
			printf("[%s]:The cycles in %5.2f (Hz):%5.2f (ms) \n", threadname, 1/elapsed, elapsed*1000);
			count[id] = 0;
	}	
}

double get_time_interval(short id)
{
	struct timespec		ts;
    static struct timespec  ts_p[5];
    double 			nsec,elapsed;


    clock_gettime(CLOCK_REALTIME, &ts);
    nsec    = ts.tv_nsec- ts_p[id].tv_nsec;
    elapsed = nsec*1.0e-9;
    
    if(elapsed <=0) elapsed =0.0;        
    ts_p[id]= ts;
   
    return elapsed;	
}	

double getTime(void)
{
  	struct timespec 		t;
  	static struct timespec	tset;
    double 					tnow;
    static 	short int		init=0;
   
    if(init==0) {
       	init = 1;
   		clock_gettime(CLOCK_REALTIME,&tset);
     	return 0.0;
    } 
	clock_gettime(CLOCK_REALTIME, &t);
    tnow = (t.tv_sec-tset.tv_sec) + 1.0e-9*(double)(t.tv_nsec - tset.tv_nsec);
    return tnow;
}		

/*#ifdef NCURSE_DISPLAY_OPTION	
static void set_colors(int pair, int foreground, int background)
{
   if (has_colors()) {
   if (pair > COLOR_PAIRS)
   	    pair = COLOR_PAIRS;
 	init_pair(pair, foreground,background);
 	wattrset(win, COLOR_PAIR(pair));
 }		    
}

static chtype use_colors(int pairs, chtype attrs)
{
   if (has_colors()) {
  	if (pairs > COLOR_PAIRS)
  	    pairs = COLOR_PAIRS;  
		attrs |= COLOR_PAIR(pairs);  
		}
  wattrset(win,attrs);
  return attrs;	    	
}
#endif*/

float dmsToRad(float degree, float minute, float second)
{
	float angle=0;
	angle=(degree+(minute/60)+(second/3600))*D2R;
	return angle;
}

float dmsToDeg(float degree, float minute, float second)
{
	float angle;
	angle=(degree+(minute/60)+(second/3600));
	return angle;
}

struct dms DegToDms(double deg)
{
	double aux,intpart;
	
	struct dms dmsPtr;
	
    aux=modf(deg,&intpart);
	dmsPtr.hours=intpart;
	
	aux=modf(aux*60,&intpart);
	dmsPtr.minutes=intpart;
	
	aux=modf(aux*60,&intpart);
	dmsPtr.seconds=intpart;
	
	return dmsPtr;
}

double piBound(double angle)
{
	double out=0;
	
	//out=(angle+pi)%(2*pi)-pi;
	
	return out;
}	

double twoPiBoound(double angle)
{
	double out=0;
	
	//out=angle%2*pi;
	
	return out;
}

// Function to substract t2-t1 (0 if negative)
/*struct timespec diff_tiempos(int tarea,struct timespec t2, struct timespec t1,int inc_perdidos)
{
	struct timespec t3;
	//caso especial donde el resultado sería negativo
	if((t2.tv_sec<t1.tv_sec)||((t2.tv_sec==t1.tv_sec)&&(t2.tv_nsec<t1.tv_nsec)))
	{	t3.tv_nsec=0;
		t3.tv_sec=0;
		return t3;
	}
	
	t3.tv_sec=t2.tv_sec-t1.tv_sec;
	if(t2.tv_nsec<t1.tv_nsec)
	{	t3.tv_sec=t3.tv_sec-1;
		t3.tv_nsec=(1000000000+t2.tv_nsec)-t1.tv_nsec;
	}
	else
		t3.tv_nsec=t2.tv_nsec-t1.tv_nsec;
	return t3;
}*/

struct timespec diff_tiempos(int tarea,struct timespec t2, struct timespec t1,int inc_perdidos)
{
	struct timespec t3;
	_uint64 auxtime;
	    auxtime = (timespec2nsec(&t2)-timespec2nsec(&t1));
		nsec2timespec(&t3,auxtime);

	return t3;
}

// Function to add Time t1+t2
struct timespec sum_tiempos(struct timespec t2, struct timespec t1)
{
	struct timespec t3;	
	/*t3.tv_nsec=t2.tv_nsec+t1.tv_nsec;
	if(t3.tv_nsec >= 1000000000)
	{	t3.tv_nsec=t3.tv_nsec-1000000000;
		t3.tv_sec=t2.tv_sec+t1.tv_sec+1;
	}
	else
		t3.tv_sec=t2.tv_sec+t1.tv_sec;*/
	_uint64 auxtime;
	auxtime = (timespec2nsec(&t2)+timespec2nsec(&t1));
	nsec2timespec(&t3,auxtime);

	return t3;
}

